'' Octopilot -- GPI_IO_octopilot.spin
'' (c) 2008-2009 Ben Levitt
'' http://code.google.com/p/octopilot/
''
'' Listen to the serial GPS, remember the data, convert it from strings to ints/char as it's requested.
''
'' Based on:
'' *****************************
'' GPS routines
''  (c) 2007 Perry James Mole
''  pjm@ridge-communications.ca
'' *****************************


CON
  CR = 13                                               ' ASCII <CR>
  LF = 10                                               ' ASCII <LF>

  
VAR  
   long gps_stack[40] 
   byte GPRMCb[68],GPGGAb[80],PGRMZb[40]   
   long GPRMCa[20],GPGGAa[20],PGRMZa[20]   

   byte gps_buff[80],Rx',cksum
   long cog,cptr,ptr,arg
   long Null[1]
   byte lockId


OBJ
  uart :  "FullDuplexSerial"
  'debug:  "FullDuplexSerial"

  
PUB start(serRecv,serXmit) : okay

'' Starts uart object (at baud specified) in a cog
'' -- returns false if no cog available

  'debug.Start(31, 30, 0, 38400)
  okay := uart.start(serRecv,serXmit,0,4800)            ' for USGlobeSat model 408 mode is 0 
                                                        ' for serial PC simulator  mode is 1
  lockId := locknew
  
  return cog := cognew(readNMEA,@gps_stack) + 1 


PUB valid : val
   repeat until not lockset(lockId)
   val := byte[GPRMCa[1]]
   lockclr(lockId)
      
PUB speed : val
   repeat until not lockset(lockId)
   val := atoi(GPRMCa[6], 4)                            ' is this right?
   lockclr(lockId)

PUB heading : val
   repeat until not lockset(lockId)
   val := atoi(GPRMCa[7], 3)
   lockclr(lockId)
   
PUB dateMM : val
   repeat until not lockset(lockId)
   val := atoi(GPRMCa[8], 2)
   lockclr(lockId)
    
PUB dateDD : val
   repeat until not lockset(lockId)
   val := atoi(GPRMCa[8]+2, 2)
   lockclr(lockId)
    
PUB dateYY : val
   repeat until not lockset(lockId)
   val := atoi(GPRMCa[8]+4, 2)
   lockclr(lockId)
    
PUB altitude : val
   repeat until not lockset(lockId)
   val := atoi(GPGGAa[8], 3)
   lockclr(lockId)

PUB bar_altitude : val
   repeat until not lockset(lockId)
   val := atoi(PGRMZa[0], 4)                            ' is this right?
   lockclr(lockId)

PUB time : val
   repeat until not lockset(lockId)
   val := atoi(GPGGAa[0], 6)
   lockclr(lockId)

PUB latitude : val
   repeat until not lockset(lockId)
   val := 600_000 * atoi(GPGGAa[1], 2) + 10_000 * atoi(GPGGAa[1]+2, 2) + atoi(GPGGAa[1]+5, 4)
   if byte[GPGGAa[2]] == "S"
     val := val * -1
   lockclr(lockId)
    
PUB N_S : val
   repeat until not lockset(lockId)
   val := byte[GPGGAa[2]]
   lockclr(lockId)
     
PUB longitude : val
   repeat until not lockset(lockId)
   val := 600_000 * atoi(GPGGAa[3], 3) + 10_000 * atoi(GPGGAa[3]+3, 2) + atoi(GPGGAa[3]+6, 4)
   if byte[GPGGAa[4]] == "W"
     val := val * -1
   lockclr(lockId)
    
PUB E_W : val
   repeat until not lockset(lockId)
   val := byte[GPGGAa[4]]
   lockclr(lockId)

PUB fixqual : val
   repeat until not lockset(lockId)
   val := atoi(GPGGAa[5], 1)
   lockclr(lockId)
      
PUB satellites : val
   repeat until not lockset(lockId)
   val := atoi(GPGGAa[6], 2)
   lockclr(lockId)
    
PUB hdop : val
   repeat until not lockset(lockId)
   val := atoi(GPGGAa[7], 4)                            ' is this right?
   lockclr(lockId)

PUB vdop : val
   repeat until not lockset(lockId)
   val := atoi(GPGGAa[14], 4)                           ' is this right?
   lockclr(lockId)


PUB tx(c)
  uart.tx(c)


PUB dec(c)
  uart.dec(c)


PRI readNMEA

  Null[0] := 0
  repeat
   longfill(@gps_buff,0,20)     ' zeroes the memory space
   repeat while Rx <> "$"      ' wait for the $ to insure we are starting with
     Rx := uart.rx              ' a complete NMEA sentence
     'debug.tx(Rx)
   cptr := 0

   repeat while Rx <> CR and Rx <> LF       '  continue to collect data until the end of the NMEA sentence 
     Rx := uart.rx              '  get character from Rx Buffer
     'debug.tx(Rx)
     if Rx == ","
       gps_buff[cptr++] := 0    '  If "," replace the character with 0
     else
       gps_buff[cptr++] := Rx   '  else save the character   
   
   if gps_buff[2] == "G"             
     if gps_buff[3] == "G"            
       if gps_buff[4] == "A"            
           copy_buffer(@GPGGAb, @GPGGAa)
   
   if gps_buff[2] == "R"             
     if gps_buff[3] == "M"            
       if gps_buff[4] == "C"
           copy_buffer(@GPRMCb, @GPRMCa)
   
   if gps_buff[0] == "P"
    if gps_buff[1] == "G"  
     if gps_buff[2] == "R"
      if gps_buff[3] == "M"  
       if gps_buff[4] == "Z"
           copy_buffer(@PGRMZb, @PGRMZa)

                
PRI copy_buffer(buffer,args) | j
  repeat until not lockset(lockId)
  bytemove(buffer,@gps_buff,cptr) '  copy received data to buffer
  ptr := buffer
  arg := 0
  repeat j from 0 to cptr-2       ' build array of pointers
    if byte[ptr] == 0              ' to each
      if byte[ptr+1] == 0           ' record
         long[args][arg] := Null     ' in 
      else                            ' the
         long[args][arg] := ptr+1      ' data buffer
      arg++
    ptr++
  lockclr(lockId)
   
' now we just need to return the pointer to the desired record
          

PRI atoi(pptr,c)| sign, i                               ' convert c characters into number
  result := sign := 0
  if byte[pptr] == "-"
    sign++
    pptr++
  c--    
  repeat i from pptr to pptr+c
    if byte[i] == 0                                     ' stop if null
      quit
         
    if byte[i] == "."                                   ' stop if decimal point
      quit
    else     
      result := result * 10 + (byte[i] - "0")
  if sign == 1
    result := -result
   